#include "robot_message.h"
#include "Gimbal_Task.h"
#include "referee.h"
#include "INS_Task.h"
#include "Switch_Task.h"
//small low high
nuc_receive_data_t	nuc_receive_data;
nuc_transmit_data_t 	nuc_transmit_data;
cmd_id_queue_t cmd_id_queue;
uint8_t aim_pressing=0;
extern aim_control_t aim_control;


/*************************** SEND ********************************/

void data_update(uint8_t cmd_id)
{
    switch(cmd_id){
        case GIMBAL_AND_CONFIG_SEND_ID:
            /*  Update the value of variables here START*/
				
						// nuc_transmit_data.robot_gimbal_data_send.camera_id=0;
						//nuc_transmit_data.robot_gimbal_data_send.is_pressing=aim_pressing;
						/*8.1*/
						nuc_transmit_data.robot_gimbal_data_send.roll=		(fp32)INS_angle_deg[3];//gimbal_LK[2].INS_angle;
            nuc_transmit_data.robot_gimbal_data_send.pitch=	-	gimbal_LK[1].INS_angle;
            nuc_transmit_data.robot_gimbal_data_send.yaw	=		gimbal_LK[0].INS_angle;
				
				
						if(Game_Robot_State.robot_id==107)
            nuc_transmit_data.robot_gimbal_data_send.mode	=		0;
						else  nuc_transmit_data.robot_gimbal_data_send.mode	=		1;
				//r 0
				//b   1
				//r fu 2
				//b fu  3
				//r fufu 4
				//b fufu 5
						
            /*  Update the value of variables here END*/
            break;
        default:
            break;
    }
}

void send_data_to_nuc(uint8_t cmd_id)
{
    uint16_t len;
			usb_cdc_data.usb_cdc_send_buf[0]=0xAA;
    
    switch (cmd_id)
    {
        case GIMBAL_AND_CONFIG_SEND_ID:
//            len=sizeof(nuc_transmit_data.robot_gimbal_data_send)+4;    
//            float_to_u8(&nuc_transmit_data.robot_gimbal_data_send.pitch,t[0]);
//            float_to_u8(&nuc_transmit_data.robot_gimbal_data_send.roll,t[1]);
//            float_to_u8(&nuc_transmit_data.robot_gimbal_data_send.yaw,t[2]);
//            for(int i=0;i<3;i++){
//                for(int j=0;j<4;j++){
//                    usb_cdc_data.usb_cdc_send_buf[4+i*4+j]=t[i][j];
//                }
//            }
//            usb_cdc_data.usb_cdc_send_buf[16]=nuc_transmit_data.robot_gimbal_data_send.is_pressing;
//            usb_cdc_data.usb_cdc_send_buf[17]=nuc_transmit_data.robot_gimbal_data_send.mode;
//            for(int i=0;i<8;i++){
//                usb_cdc_data.usb_cdc_send_buf[18+i]=nuc_transmit_data.robot_gimbal_data_send.mode_config[i];
//            }
//            usb_cdc_data.usb_cdc_send_buf[26]=nuc_transmit_data.robot_gimbal_data_send.camera_id;
							/*8.1*/
							len=16;
							usb_cdc_data.usb_cdc_send_buf[0]=0xff;
							usb_cdc_data.usb_cdc_send_buf[1]= nuc_transmit_data.robot_gimbal_data_send.mode;
				memcpy((uint8_t*)&usb_cdc_data.usb_cdc_send_buf[2],(uint8_t*)&nuc_transmit_data.robot_gimbal_data_send.roll,4);
				memcpy((uint8_t*)&usb_cdc_data.usb_cdc_send_buf[6],(uint8_t*)&nuc_transmit_data.robot_gimbal_data_send.pitch,4);
				memcpy((uint8_t*)&usb_cdc_data.usb_cdc_send_buf[10],(uint8_t*)&nuc_transmit_data.robot_gimbal_data_send.yaw,4);
							usb_cdc_data.usb_cdc_send_buf[14]=0x00;
							usb_cdc_data.usb_cdc_send_buf[15]=0x0d;
            break;
        default:
            break;
    }
	
//		usb_cdc_data.usb_cdc_send_buf[3]=cmd_id;
//		usb_cdc_data.usb_cdc_send_buf[1]=(len);
//    usb_cdc_data.usb_cdc_send_buf[2]=((len>>8));
//    usb_cdc_data.usb_cdc_send_buf[len-1]=CRC_Calculation(usb_cdc_data.usb_cdc_send_buf,len-1);
    usb_data_send(usb_cdc_data.usb_cdc_send_buf,len);
	
		//send
		// uint8_t 0xff	
		// uint8_t  mode
		// fp32    roll	
		// fp32   pitch	
		// fp32    yaw	
		// uint8_t 0x00	
		// uint8_t 0x0d	
}

//AA 0C 00 13 00 01 02 03 04 05 06 07 08 09 0A XX
/*************************** SEND ********************************/
/*************************** RECV ********************************/
void Nuc_data_unpacked()
{
//	memcpy(dectt,usb_cdc_data.usb_cdc_rx_buf,16);
//    if(usb_cdc_data.usb_cdc_rx_flag==1)
//			{
			
					nuc_receive_data.aim_data_received.target_rate=usb_cdc_data.usb_cdc_rx_buf[1];
															memcpy(&nuc_receive_data.aim_data_received.pitch,&usb_cdc_data.usb_cdc_rx_buf[2],4);
															memcpy(&nuc_receive_data.aim_data_received.yaw,&usb_cdc_data.usb_cdc_rx_buf[6],4);
															memcpy(&nuc_receive_data.aim_data_received.success,&usb_cdc_data.usb_cdc_rx_buf[10],4);
																nuc_receive_data.aim_data_received.target_number=usb_cdc_data.usb_cdc_rx_buf[14];
												if(nuc_receive_data.aim_data_received.success>0)//&&(nuc_receive_data.aim_data_received.success<4||aim_control.aim_debug_flag==1)) 
													nuc_receive_data.aim_data_received.success=1;
												else  nuc_receive_data.aim_data_received.success=0;
//				}


		usb_cdc_data.usb_cdc_rx_flag=0;
//    }
}

/*************************** RECV ********************************/
void float_to_u8(float* float_in,uint8_t* u8_out)
{
    uint8_t farray[4];
    *(float*)farray=*float_in;
    u8_out[3]=farray[3];
    u8_out[2]=farray[2];
    u8_out[1]=farray[1];
    u8_out[0]=farray[0];
}
void u8_to_float(uint8_t* datain,float* dataout)
{
    *dataout=*(float *)datain;
}

void cmd_id_task_create(uint8_t cmd_id,uint16_t freq)
{
	cmd_id_queue.cmd_id_queue[cmd_id_queue.total_num]=cmd_id;
	cmd_id_queue.cmd_id_frq[cmd_id_queue.total_num]=1000/(freq%1000);
  cmd_id_queue.total_num+=1;
}
void cmd_id_queue_handle()
{
		for(int i=0;i<cmd_id_queue.total_num;i++){
				if(cmd_id_queue.now_pos%cmd_id_queue.cmd_id_frq[i]==0){
					data_update(cmd_id_queue.cmd_id_queue[i]);
					send_data_to_nuc(cmd_id_queue.cmd_id_queue[i]);
				}
		}
    cmd_id_queue.now_pos++;
    if(cmd_id_queue.now_pos>=1000){
        cmd_id_queue.now_pos=0;
    }
}

void cmd_id_init()
{
    cmd_id_queue.total_num=0;
    cmd_id_queue.now_pos=0;
}
uint8_t CRC_Calculation(uint8_t *ptr, uint16_t len) 
{
	uint8_t crc = 0xff;
	while (len--) {
		crc = CRC08_Table[crc ^ *ptr++];
	}
	return crc;
}
